/**************************************************************************************
*
*	Copyright (C) 2009 - 2012 Brent Andrew O. Chua - See LICENSE.txt for more info.
*		
*	Filename	-	PhysicsSystem.cpp
*
*	Comments	-	See PhysicsSystem.h
*
**************************************************************************************/
#include "../Include/PhysicsSystem.h"

#include "../Include/Map.h"
#include "../Include/Matrix4x4.h"
#include "../Include/String.h"
#include "../Include/Actor.h"
#include "../Include/ComponentPhysics.h"
#include "../Include/ComponentModelFilter.h"
#include "../Include/Model.h"
#include "../Include/EventTransform.h"

///////////////////////Bullet Physics////////////////////
#include "btBulletDynamicsCommon.h"

#ifdef PSX_DEBUG
#pragma comment( lib, "BulletDynamicsd.lib" )
#pragma comment( lib, "BulletCollisiond.lib" )
#pragma comment( lib, "LinearMathd.lib" )
#else 
#pragma comment( lib, "BulletDynamics.lib" )
#pragma comment( lib, "BulletCollision.lib" )
#pragma comment( lib, "LinearMath.lib" )
#endif
/////////////////////////////////////////////////////////

namespace Pulse
{
	class BulletPhysics;

	void PhysicsDummyFunction( void )
	{
		btBoxShape *pBox = new btBoxShape( btVector3( 1, 1, 1 ) );
		delete pBox;
	}


	/////////////////////////////////////////////////////////////////////////////
	// struct ActorMotionState						
	//
	// Interface that Bullet uses to communicate position and orientation changes
	//   back to the game.  note:  this assumes that the actor's center of mass
	//   and world position are the same point.  If that was not the case,
	//   an additional transformation would need to be stored here to represent
	//   that difference.
	//
	struct PulseBTMotionState : public btMotionState
	{
		PulseBTMotionState( Actor *const pActorOwner, Matrix4x4 const & startingTransform )
			: m_pActor( pActorOwner ), m_transform( startingTransform ) { }

		PulseBTMotionState( Actor * const pActorOwner, Matrix4x4 const * pStartingTransform )
			:m_pActor( pActorOwner ), m_transform( *pStartingTransform ) { }

		Actor * const GetActor( void ) { return m_pActor; }

		// btMotionState interface:  Bullet calls these
		virtual void getWorldTransform( btTransform& worldTrans ) const;

		virtual void setWorldTransform( const btTransform& worldTrans );

	private:

		void ToBTTransform( const Matrix4x4 * const pMatrix, btTransform * const pOut ) const;

		void ToPulseTransform( const btTransform *pTransform, Matrix4x4 * const pOut );

		Actor * const m_pActor;

		Matrix4x4 m_transform;
	};

	// btMotionState interface:  Bullet calls these
	void PulseBTMotionState::getWorldTransform( btTransform& worldTrans ) const
	{ 
		//ToBTTransform( &m_transoform, &worldTrans ); 

		ToBTTransform( m_pActor->GetTransformWorld(), &worldTrans );
	}

	void PulseBTMotionState::setWorldTransform( const btTransform& worldTrans )
	{ 
		ToPulseTransform( &worldTrans, &m_transform );

		//EventTransform newTransform;

		//newTransform.SetTransform( 

		Matrix4x4 localTransform = *m_pActor->GetTransformLocal();
		localTransform.Invert();

		Matrix4x4 local;

		// BUG: The orientation doesn't keep track of scale. 
		// So reset the scale back after setting the world matrix.
		const Vector3 currentScale = *m_pActor->GetScale();
		m_pActor->SetTransformWorld( &m_transform );
		m_pActor->SetScale( &currentScale );
	}

	void PulseBTMotionState::ToBTTransform( const Matrix4x4 * const pMatrix, btTransform * const pOut ) const
	{
		btMatrix3x3 bulletRotation;
		btVector3 bulletPosition;

		for ( INT i = 0; i < 3; ++i )
		{
			for ( INT j = 0; j < 3; ++j )
			{
				 bulletRotation[i][j] = (*pMatrix)[j][i];
			}
		}

		for ( INT i = 0; i < 3; ++i )
		{
			bulletPosition[i] = (*pMatrix)[3][i];
		}

		pOut->setBasis( bulletRotation );
		pOut->setOrigin( bulletPosition );
	}

	void PulseBTMotionState::ToPulseTransform( const btTransform *pTransform, Matrix4x4 * const pOut )
	{
		const btMatrix3x3 &bulletRotation = pTransform->getBasis();
		const btVector3 &bulletPosition = pTransform->getOrigin();
		Matrix4x4 orientation;

		for ( INT i = 0; i < 3; ++i )
		{
			for ( INT j = 0; j < 3; ++j )
			{
				orientation[i][j] = bulletRotation[j][i];
			}

			orientation[i][3] = 0.0f;
		}

		for ( INT i = 0; i < 3; ++i )
		{
			orientation[3][i] = bulletPosition[i];
		}
		orientation[3][3] = 1.0f;

		*pOut = orientation;
	}

	class BulletPhysics :public IPhysicsSystem
	{
	public:

		BulletPhysics( void );

		virtual ~BulletPhysics( void );

		virtual EErrorCode::Type Initialize( void );

		virtual void Update( const FLOAT dt );

		// TODO: Add more different types of colliders
		EErrorCode::Type ConfigureBoxCollider( ComponentPhysics *pCompPhys ) sealed;

		virtual void RemoveComponentCollider( ComponentPhysics *pCompPhys );

	private:

		friend static void BulletTickCallback( btDynamicsWorld * const world, btScalar const timeStep );
		void OnTickCallback( btDynamicsWorld * const pWorld, btScalar const timeStep );

		void AddNewRigidBody( ComponentPhysics *pCompPhys, btCollisionShape *pShape, const FLOAT mass, const PhysicsMaterial *pMaterial );

		void Cleanup( void );

	private:

		typedef Map<ActorID, btRigidBody*> ActorToBTRigidBodyMap;
		//typedef Map<btRigidBody *, ActorID> BTRigidBodyToActorMap;
		typedef PSX_Pair< const btRigidBody*, const btRigidBody*> BTCollisionPair;

		btDynamicsWorld					*m_pWorld;
		btBroadphaseInterface			*m_pBroadphase;
		btCollisionDispatcher			*m_pDispatcher;
		btConstraintSolver				*m_pSolver;
		btDefaultCollisionConfiguration	*m_pCollisionConfiguration;
		//BulletDebugDrawer				*m_pDebugDrawer;

		ActorToBTRigidBodyMap	m_actorToRigidBodyMap;
		//BTRigidBodyToActorMap	m_rigidBodyToActorMap;
	};

	BulletPhysics::BulletPhysics( void )
	{
		m_pWorld = PSX_NULL;
		m_pBroadphase = PSX_NULL;
		m_pDispatcher = PSX_NULL;
		m_pSolver = PSX_NULL;
		m_pCollisionConfiguration = PSX_NULL;
	}

	BulletPhysics::~BulletPhysics( void )
	{
		Cleanup();
	}

	EErrorCode::Type BulletPhysics::Initialize( void )
	{
		if ( m_pWorld )
		{
			PSX_PushWarning( "Already initialized physics system." );
			return EErrorCode::OKAY;
		}

		m_pBroadphase = new btDbvtBroadphase;
		m_pCollisionConfiguration = new btDefaultCollisionConfiguration; // For configuring the narrowphase collision(dispatcher)
		m_pDispatcher = new btCollisionDispatcher( m_pCollisionConfiguration );
		m_pSolver = new btSequentialImpulseConstraintSolver;

		// Finally create the world
		m_pWorld = new btDiscreteDynamicsWorld( m_pDispatcher, m_pBroadphase, m_pSolver, m_pCollisionConfiguration);
		m_pWorld->setGravity( btVector3(0,-9.80f,0) );

		m_pWorld->setInternalTickCallback( BulletTickCallback );
		m_pWorld->setWorldUserInfo( this );

		// NOTE: Additional initialization

		return EErrorCode::OKAY;
	}

	void BulletTickCallback( btDynamicsWorld * const world, btScalar const timeStep )
	{
		((BulletPhysics*)world->getWorldUserInfo())->OnTickCallback( world, timeStep );
	}
	
	void BulletPhysics::OnTickCallback( btDynamicsWorld * const pWorld, btScalar const timeStep )
	{

		btDispatcher * const pDispatcher = pWorld->getDispatcher();
		const int numManifolds = pDispatcher->getNumManifolds();

		// 
		for ( int i = 0; i < numManifolds; ++i )
		{
			
			// get the "manifold", which is the set of data corresponding to a contact point
			//   between two physics objects
			btPersistentManifold const * const pManifold = pDispatcher->getManifoldByIndexInternal( i );

			// get the two bodies used in the manifold.  Bullet stores them as void*, so we must cast
			//  them back to btRigidBody*s.  Manipulating void* pointers is usually a bad
			//  idea, but we have to work with the environment that we're given.  We know this
			//  is safe because we only ever add btRigidBodys to the simulation
			btRigidBody const * const pBody0 = static_cast<btRigidBody const *>(pManifold->getBody0());
			btRigidBody const * const pBody1 = static_cast<btRigidBody const *>(pManifold->getBody1());

			// always sort in a predictable order
			bool const bSwapped = pBody0 > pBody1;

			btRigidBody const * const pSortedBodyA = bSwapped ? pBody1 : pBody0;
			btRigidBody const * const pSortedBodyB = bSwapped ? pBody0 : pBody1;
			
			//BTCollisionPair const collisionPair = BTCollisionPair::PSX_Pair( pSortedBodyA, pSortedBodyB );
			//BTCollisionPair const collisionPair = BTCollisionPair::PSX_Pair( pBody0, pBody1 );

			// Send collision event to the physics component
			{
				// Construct Collision shared data
				{
					ComponentPhysics *pCompPhys0 = (ComponentPhysics *)pSortedBodyA->getUserPointer();
					ComponentPhysics *pCompPhys1 = (ComponentPhysics *)pSortedBodyB->getUserPointer();
					CollisionData::SharedDataPtr pSharedData = new CollisionData::SharedData;

					{
						CollisionData::CollisionPointList collisionPoints;
						Vector3 sumNormalForce = Vector3::ZERO_VECTOR;
						Vector3 sumFrictionForce = Vector3::ZERO_VECTOR;
						const int numContacts = pManifold->getNumContacts();

						for ( int contactIndex = 0; i < numContacts; ++i )
						{
							const btManifoldPoint & point = pManifold->getContactPoint( contactIndex );
							const btVector3 &worldPos = point.getPositionWorldOnB();
							const btVector3 normalForce = point.m_combinedRestitution * point.m_normalWorldOnB;
							const btVector3 fricionForce = point.m_combinedFriction * point.m_lateralFrictionDir1;

							collisionPoints.PushBack( Vector3( worldPos.x(), worldPos.y(), worldPos.z() ) );
							sumNormalForce += Vector3( normalForce.x(), normalForce.y(), normalForce.z() );
							sumFrictionForce += Vector3( fricionForce.x(), fricionForce.y(), fricionForce.z() );
						}

						pSharedData->collisionPoints = PSX_Move( collisionPoints );
						pSharedData->totalNormalForce = PSX_Move( sumNormalForce );
						pSharedData->totalFrictionForce = PSX_Move( sumFrictionForce );
					}

					// Send collision data to the physics component involved
					{
						{
							CollisionData collisionData;

							collisionData.pContactBody = pCompPhys1;
							collisionData.pSharedData = pSharedData;

							pCompPhys0->OnManifold( collisionData );
						}

						{
							CollisionData collisionData;

							collisionData.pContactBody = pCompPhys0;
							collisionData.pSharedData = pSharedData;

							pCompPhys1->OnManifold( collisionData );
						}
					}
				}
			}
		}
	}

	void BulletPhysics::Update( const FLOAT dt )
	{
		// Will update a maximum of 4 times.
		m_pWorld->stepSimulation( dt, 4 );
	}

	EErrorCode::Type BulletPhysics::ConfigureBoxCollider( ComponentPhysics *pCompPhys )
	{
		if ( pCompPhys == PSX_NULL )
		{
			PSX_PushError( "Parameter pActor must not be null." );
			return EErrorCode::INVALID_PARAMETER;
		}


		// Before anything else, remove the collider if it already has one.
		RemoveComponentCollider( pCompPhys );

		ComponentModelFilter *pCompModel = pCompPhys->GetComponent<ComponentModelFilter>();

		// If we have a model filter we'll base our collision shape size to it. Otherwise just the default size 1.
		if ( pCompModel )
		{
			btVector3 boxHalfExtents( 0.5f, 0.5f, 0.5f );

			// Compute Half Extents first
			{
				// TODO: We may need to do a pointer address check to see that we're getting valid data from these...
				ComponentTransform *pTransform = pCompPhys->GetComponent<ComponentTransform>();
				pTransform->Update( 0.0f ); // Force update transforms

				const Vector3 scale = *pTransform->GetScale();
				Model * const pModel = pCompModel->GetModel();
				Geometry * const pGeometry = pModel->GetGeometry();
				const GeometryDesc *pDesc = pGeometry->GetDesc();
				const VertexSource *pStreamPos = pDesc->GetVertexSource( EVertexElement::POSITION );
				const Vector3 *pPositions = pStreamPos->GetAsPositions();
				const SIZE_T numVertices = pStreamPos->GetElementCount();
				btConvexHullShape extentsComputer;
			
				// NOTE: May need to take scaling into account?
				for ( SIZE_T i = 0; i < numVertices; ++i )
				{
					const Vector3 *pCurrPos = pPositions + i;
					extentsComputer.addPoint( btVector3( pCurrPos->x * scale.x, pCurrPos->y * scale.x, pCurrPos->z  * scale.x ) );
				}

				btVector3 aabbMin( 0.0f, 0.0f, 0.0f ), aabbMax( 0.0f, 0.0f, 0.0f );
				extentsComputer.getAabb( btTransform::getIdentity(), aabbMin, aabbMax ); 
				btVector3 extentsDelta = aabbMax - aabbMin;

				boxHalfExtents = extentsDelta / 2.0f; 
			
				pGeometry->Release();
				pModel->Release();
			}
			
			btBoxShape *pBoxShape = new btBoxShape( boxHalfExtents );

			const FLOAT mass = 50.0f;
			PhysicsMaterial material( 0.65f, 0.5f );

			AddNewRigidBody( pCompPhys, pBoxShape, mass, &material );
		}

		return EErrorCode::OKAY;
	}

	void BulletPhysics::RemoveComponentCollider( ComponentPhysics *pCompPhys )
	{
		ActorToBTRigidBodyMap::Iterator findIter = m_actorToRigidBodyMap.Find( pCompPhys->GetOwnerUnsafe()->GetID() );

		if ( findIter != m_actorToRigidBodyMap.IteratorEnd() )
		{
			btRigidBody *pRigidBody = findIter->second;

			// Remove the object from the physics sim
			m_pWorld->removeCollisionObject( pRigidBody );

			// Do some last processing of the physics component
			pCompPhys->OnRemoveCollider();

			// Remove mapping
			m_actorToRigidBodyMap.Remove( findIter );

			// Delete components of the object
			delete pRigidBody->getMotionState();
			delete pRigidBody->getCollisionShape();

			// Finally remove self
			delete pRigidBody;
		}
		else
		{
			//PSX_PushWarning( "Rigidbody of physics component not found." );
		}
	}

	void BulletPhysics::AddNewRigidBody( ComponentPhysics *pCompPhys, btCollisionShape *pShape, const FLOAT mass, const PhysicsMaterial *pMaterial )
	{
		btVector3 localIntertia( 0.0f, 0.0f, 0.0f );
		
		// localInertia defines how the object's mass is distributed
		if ( mass > 0.0f )
		{
			pShape->calculateLocalInertia( mass, localIntertia );
		}

		ComponentTransform *pCompTransform = pCompPhys->GetComponent<ComponentTransform>();

		Matrix4x4 motionStateTransform;
		motionStateTransform.BuildRotationPitchYawRoll( pCompTransform->GetTransformWorld()->GetPitchYawRoll() );
		motionStateTransform.SetTranslation( pCompTransform->GetTransformWorld()->GetTranslation() );

		PulseBTMotionState *pNewMotionState = new PulseBTMotionState( pCompPhys->GetOwnerUnsafe(),
			pCompTransform->GetTransformWorld() );

		btRigidBody::btRigidBodyConstructionInfo rbInfo( mass, pNewMotionState, pShape, localIntertia );

		// set up the material properties
		rbInfo.m_restitution = pMaterial->bounciness;
		rbInfo.m_friction = pMaterial->friction;

		btRigidBody * const pNewRigidBody = new btRigidBody( rbInfo );
		pNewRigidBody->setUserPointer( pCompPhys );


		m_pWorld->addRigidBody( pNewRigidBody );

		// add it to the mappings
		{
			Actor *pActor = pCompPhys->GetOwnerUnsafe();
			
			m_actorToRigidBodyMap[pActor->GetID()] = pNewRigidBody;
			//m_rigidBodyToActorMap[pNewRigidBody] = pActor->GetID();
		}
	}

	void BulletPhysics::Cleanup( void )
	{
		// TODO: Release all resources here...
		//remove the rigidbodies from the dynamics world and delete them
		//for ( int i = m_pWorld->getNumCollisionObjects()-1; i>=0 ;i--)
		//{
		//	btCollisionObject* obj = m_pWorld->getCollisionObjectArray()[i];
		//	btRigidBody* body = btRigidBody::upcast(obj);
		//	if (body && body->getMotionState())
		//	{
		//		delete body->getMotionState();
		//	}
		//	m_pWorld->removeCollisionObject( obj );
		//	delete obj;
		//}



		// Clean up behind ourselves like good little programmers
		delete m_pWorld;
		delete m_pSolver;
		delete m_pDispatcher;
		delete m_pCollisionConfiguration;
		delete m_pBroadphase;
	}



	/////////////////////////////////////////////////////
	IPhysicsSystem * CreatePhysicsSystem( void )
	{
		return new BulletPhysics;
	}

	void DestroyPhysicsSystem( IPhysicsSystem *pPhysicsSys )
	{
		delete pPhysicsSys;
	}

}